#include "lpf2.h"

/**
 * @brief  互补滤波
 * @param  wc : 截止频率
 * @param  dt : 周期
 * @retval void
 */
void lpf2_init(lpf2_t *hlpf2, float wc,float dt)
{
    float fr = (1.0f/dt)/wc;
	float ohm = tanf(M_PI_F/fr);
	float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm;
	hlpf2->b0 = ohm*ohm/c;
	hlpf2->b1 = 2.0f*hlpf2->b0;
	hlpf2->b2 = hlpf2->b0;
	hlpf2->a1 = 2.0f*(ohm*ohm-1.0f)/c;
	hlpf2->a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
	
    hlpf2->yn[1] = 0;
    hlpf2->yn[0] = 0;
    
    hlpf2->xn[2] =  0;
    hlpf2->xn[1] =  0;
    hlpf2->xn[0] =  0;
}

/**
 * @brief  一阶低通滤波
 * @param  in:输入
 * @retval void
 */
float lpf2_apply(lpf2_t *hlpf2, float xn)
{
    hlpf2->xn[0] = xn;

    // Y(n) = b0*X(n) + b1*X(n-1) + b1*X(n-2) + a0*Y(n-1) + a1*Y(n-2)
    float yn = hlpf2->b0 * hlpf2->xn[0] + hlpf2->b1 * hlpf2->xn[1] + hlpf2->b2 * hlpf2->xn[2] - \
               hlpf2->a1 * hlpf2->yn[0] - hlpf2->a2 * hlpf2->yn[1]; 

    hlpf2->yn[1] = hlpf2->yn[0];
    hlpf2->yn[0] = yn;
    
    hlpf2->xn[2] =  hlpf2->xn[1];
    hlpf2->xn[1] =  hlpf2->xn[0];

    return yn;
}

/******************* (C) COPYRIGHT 2024 FUYEBING*****END OF FILE************/